#一、引入库
import sensor, image, time,car
from math import asin,acos,atan,pi,sin,cos
from time import sleep as 延迟

#二、声明
屏幕=car.screen() #声明屏幕
图像=sensor.snapshot()    #通过感光器获取声明一个图像
图像.clear()  #图像清空
图像.draw_rectangle(18,18,284,144)    #画方框触控区
滑块=car.touch_slider(图像,18,184,284,40)   #声明滑块
屏幕.display(图像)  #屏幕显示图像
舵机=car.servo_motor()    #声明舵机

#三、常量
底座端口=1
大臂端口=2
小臂端口=0
各轴范围=((91,153),(30,93),(25,95)) #底座，大臂 小臂，0度和90度的控制数值

底座高度=0.8*8.5    #高度单位cm。0.8是积木孔距
大臂长度=0.8*11
小臂长度=0.8*11
臂向补偿=3.6
高度补偿=0.8*2.5

#四、控制三路舵机：
#机械臂控制函数，输入角度(弧度制)，使机械臂各轴完成控制。可以将弧度转换为控制值，且限制范围。
#弧度制与角度值转换：pi=180角度≈3.14弧度
def 机械臂控制(底座角,大臂角,小臂角):#以弧度制控制
    global 底座端口,大臂端口,小臂端口,各轴范围
    底座角=各轴范围[0][0]+底座角*((各轴范围[0][1]-各轴范围[0][0])/(pi/2))#把弧度转化为舵机控制值,下同
    if 底座角>各轴范围[0][1]:  #限制最大值最小值，下同
        底座角=各轴范围[0][1]
    elif 底座角<-各轴范围[0][1]:
        底座角=-各轴范围[0][1]
    大臂角=各轴范围[1][1]-大臂角*((各轴范围[1][1]-各轴范围[1][0])/(pi/2))
    if 大臂角>各轴范围[1][1]:
        大臂角=各轴范围[1][1]
    elif 大臂角<各轴范围[1][0]:
        大臂角=各轴范围[1][0]
    小臂角=各轴范围[2][1]-小臂角*((各轴范围[2][1]-各轴范围[2][0])/(pi/2))
    if 小臂角>各轴范围[2][1]:
        小臂角=各轴范围[2][1]
    elif 小臂角<各轴范围[2][0]:
        小臂角=各轴范围[2][0]
    舵机.degree(底座端口,底座角)
    舵机.degree(大臂端口,大臂角)
    舵机.degree(小臂端口,小臂角)
    #print(str(底座角)+','+str(大臂角)+','+str(小臂角))

#五：核心知识：三轴机械臂逆运算。只要初中三角函数！
def 机械臂逆运算(x,y,z,显示轨迹=True):
    global 底座高度,大臂长度,小臂长度,臂向补偿,高度补偿,图像  #声明全局变量
    if 显示轨迹:
        图像.draw_circle(round(x*17.5+160),round(160-y*17.5),2,color=(255,0,0))
        屏幕.display(图像)
#其他补偿量
    y = y+10 #原点补偿，y轴小靠近底座效果不佳，且笔和臂容易冲突,且第一组计算中作为被除数不能为0。
    z = z+高度补偿  #补偿机械臂夹笔的框架和笔的尺寸
    z = z+y*0.1 #补偿臂向前伸时下垂
    x = x*(1.16-y*0.01)  #补偿因为旷量减少的x轴长度,随着y轴增加，所需补偿量减小
#第一步、计算底座角
    #通过落点在x轴的范围，分为三种情况：
    底座角=atan(x/y)  #pi/2=90°角。x、y值通过atan求角度。
#第二步、计算大臂角
    垂直投影=(x**2+y**2)**0.5-臂向补偿  #垂直投影，为四边形的底边
    短边=abs(底座高度-z)  #下边三角形的短边
    斜边=(短边**2+垂直投影**2)**0.5 #下边三角形的斜边（辅助线）
    #通过余弦定理求角。余弦定理：cos∠C=(a²+b²-c²)/(2ab)
    大臂角一=acos((大臂长度**2+斜边**2-小臂长度**2)/(2*大臂长度*斜边))  #大臂角在下三角形的部分
    #以底座高度和z轴长度判断，分为三种情况：
    if 底座高度==z:
        大臂角二=0
    elif 底座高度>z:
        大臂角二=atan(垂直投影/短边)-pi/2 #通过tan求角
    elif 底座高度<z:
        大臂角二=atan(短边/垂直投影)
    大臂角 = 大臂角一 + 大臂角二   #两个角的总和为大臂角
#第三步、计算小臂角
    #此时已知小臂所在三角形的三边长。通过余弦定理求小臂角。
    #+大臂角，因为结构上，小臂连杆在大臂上，所以他们角度是相关的
    #-pi/2（90°），因为小臂舵机是反向摆放的，它的0-90度对应的控制数值是反的
    小臂角 = acos((大臂长度**2+小臂长度**2-斜边**2)/(2*大臂长度*小臂长度))+大臂角-pi/2
#最后、将三个角，输入机械臂控制函数
    机械臂控制(底座角,大臂角,小臂角)

#六、抬笔后的缓慢移动程序。此处可不用理解。仅为演示效果而做。
上一次x=0
上一次y=0
上一次z=0
#这个函数为了降低移动速度，防止抖动
def 长距离缓慢移动(x,y,z,显示轨迹=False):
    global 上一次x,上一次y,上一次z
    #两次移动之间的直线长度作为系数
    比例系数=((x-上一次x)**2+(y-上一次y)**2)**0.5*2
    if 比例系数!=0:
        if 比例系数<1:
            比例系数=1
        else:
            比例系数=round(比例系数)
        x步进=(x-上一次x)/比例系数
        y步进=(y-上一次y)/比例系数
        for n in range(比例系数+1):
            机械臂逆运算(上一次x+x步进*n,上一次y+y步进*n,上一次z,显示轨迹=显示轨迹)
            延迟(0.05)
        延迟(0.2)
        z步进=(z-上一次z)/5
        for n in range(6):
            机械臂逆运算(x,y,上一次z+z步进*n,显示轨迹=显示轨迹)
            延迟(0.05)
    elif z!=上一次z:
        z步进=(z-上一次z)/5
        for n in range(6):
            机械臂逆运算(x,y,上一次z+z步进*n,显示轨迹=显示轨迹)
            延迟(0.05)
    上一次x=x
    上一次y=y
    上一次z=z

#七、主循环
机械臂逆运算(0,0,2.5) #执行机械臂初始位置
目标x=0   #一些主循环所用变量
目标y=0
目标z=0
上一次目标z=0
首次落笔=True
落笔高度=1.4
抬笔后最低高度=2.5
while True:
    屏幕.get_touch()  #获取触摸
    if 屏幕.touch_exist():    #如果存在触摸
        if 20<屏幕.touch_x()<300 and 20<屏幕.touch_y()<160: #触摸在方框区域内
            目标x=(屏幕.touch_x()-160)/17.5 #触摸数值对应落笔坐标的换算
            目标y=(160-屏幕.touch_y())/17.5
            目标z=落笔高度    #高度改为落笔高度
            if 首次落笔==True:  #首次落笔，从上一次位置，缓慢移动至当前位置
                长距离缓慢移动(目标x,目标y,目标z,显示轨迹=False)
                首次落笔=False
            else:   #非首次落笔，直接按目标坐标移动
                机械臂逆运算(目标x,目标y,目标z,显示轨迹=True)
            上一次x=目标x    #装填上次坐标，为了缓慢移动函数
            上一次y=目标y
            上一次z=目标z
        else:       #没有触屏数据
            首次落笔=True   #首次落笔重置
            目标z=滑块.value(图像)/10+抬笔后最低高度 #根据滑块设置Z轴
    else:       #没有触屏数据
        首次落笔=True   #首次落笔重置
        目标z=滑块.value(图像)/10+抬笔后最低高度 #根据滑块设置Z轴

    if 上一次目标z != 目标z:   #如果z轴数值有变动（拖拽了滑块）
        上一次目标z = 目标z    #记录上一次z轴
        机械臂逆运算(目标x,目标y,目标z,显示轨迹=False)  #执行z轴的移动
        上一次x=目标x    #装填上次值，为了缓慢移动函数
        上一次y=目标y
        上一次z=目标z
        屏幕.display(图像)  #显示图像，刷新滑块
